#include <iostream>

#include <boost/format.hpp>
#include <boost/units/io.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/plane_angle.hpp>

#include "brics_actuator/JointPositions.h"

#include "youbot_relay/move_youbot_arm_server.h"

using namespace std;

std::ostream& operator<<(ostream& stream, const boost::array<float, 5>& p)
{
  return stream << boost::format("[%.3f; %.3f; %.3f; %.3f; %.3f]") % p[0] % p[1] % p[2] % p[3] % p[4];
}

namespace move_youbot_arm
{

MoveYoubotArmServer::MoveYoubotArmServer(ros::NodeHandle n) :
  node_(n), action_server_(NULL)
{
  ROS_INFO("Creating action server.");
  action_server_ = new MoveYoubotArmActionServer(ros::NodeHandle(), "move_youbot_arm",
                                                 boost::bind(&MoveYoubotArmServer::execute, this, _1), false);

  ROS_INFO("Creating arm position publisher.");
  arm_position_publisher_ = node_.advertise<brics_actuator::JointPositions>("arm_1/arm_controller/position_command", 1);

  ROS_INFO("Creating gripper position publisher.");
  gripper_position_publisher_ = node_.advertise<brics_actuator::JointPositions>("arm_1/gripper_controller/position_command", 1);

  ROS_INFO("Subscribing to joint state updates.");
  joint_state_subscriber_ = node_.subscribe("youbot_state", 100, &MoveYoubotArmServer::jointStateUpdate, this);

  action_server_->start();
}

MoveYoubotArmServer::~MoveYoubotArmServer()
{
  ROS_INFO("Deleting action server.");
  if (action_server_ != NULL)
    delete action_server_;
}

void MoveYoubotArmServer::jointStateUpdate(const sensor_msgs::JointState& state)
{
  // TODO: do something about joint states
}

void MoveYoubotArmServer::publishArmPosition(const ArmPosition& position)
{
  brics_actuator::JointPositions command;
  vector <brics_actuator::JointValue> arm_joint_positions;

  arm_joint_positions.resize(position.size());

  for (unsigned int i = 0; i < position.size(); ++i)
  {
    arm_joint_positions[i].joint_uri = boost::str(boost::format("arm_joint_%d") % (i + 1));
    arm_joint_positions[i].value = position[i];
    arm_joint_positions[i].unit = boost::units::to_string(boost::units::si::radians);
  };

  command.positions = arm_joint_positions;
  arm_position_publisher_.publish(command);
}

void MoveYoubotArmServer::publishGripperPosition(float left, float right)
{
  brics_actuator::JointPositions command;
  vector <brics_actuator::JointValue> gripper_joint_positions;

  gripper_joint_positions.resize(2);

  gripper_joint_positions[0].joint_uri = "gripper_finger_joint_l";
  gripper_joint_positions[0].value = left;
  gripper_joint_positions[0].unit = boost::units::to_string(boost::units::si::meter);

  gripper_joint_positions[1].joint_uri = "gripper_finger_joint_r";
  gripper_joint_positions[1].value = right;
  gripper_joint_positions[1].unit = boost::units::to_string(boost::units::si::meter);

  command.positions = gripper_joint_positions;

  gripper_position_publisher_.publish(command);
}

void MoveYoubotArmServer::execute(const youbot_relay::MoveYoubotArmGoalConstPtr& move_youbot_arm_goal)
{
  ROS_INFO("Move arm action callback.");
  ROS_INFO_STREAM("Requested arm joint states: " << move_youbot_arm_goal->arm_joint);
  ROS_INFO_STREAM("Requested gripper position: " << move_youbot_arm_goal->gripper_position);
  publishArmPosition(move_youbot_arm_goal->arm_joint);
  publishGripperPosition(move_youbot_arm_goal->gripper_position, move_youbot_arm_goal->gripper_position);
  action_server_->setSucceeded(youbot_relay::MoveYoubotArmResult(), "Goal reached.");
}
}
;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "move_youbot_arm_server");
  ros::NodeHandle node;

  move_youbot_arm::MoveYoubotArmServer move_youbot_arm_server(node);
  ros::spin();
  return 0;
}
